//
//  Scout.h
//  symbrion
//
//  Created by Berend Weel on 4/23/13.
//  Copyright (c) 2013 Berend Weel. All rights reserved.
//

#ifndef symbrion_Scout_h
#define symbrion_Scout_h

#include "Scout_noWheel.h"

class Scout : public Scout_noWheel {
public:
    Scout() :
    wheel1(NULL),
    wheel2(NULL),
    wheel3(NULL),
    wheel4(NULL),
    wheel5(NULL),
    wheel6(NULL),
    wheel7(NULL),
    wheel8(NULL) {
        
    }
    
    virtual void initialiseHardware() {
        KIT_noWheel::initialiseHardware();
        
        initialiseServos();
    }
    
private:
    void initialiseServos(){
        // Wheel Servo's
        wheel1 = getServo("wheel1");
		wheel2 = getServo("wheel2");
		wheel3 = getServo("wheel3");
		wheel4 = getServo("wheel4");
		wheel5 = getServo("wheel5");
		wheel6 = getServo("wheel6");
		wheel7 = getServo("wheel7");
		wheel8 = getServo("wheel8");
        
        wheel1->setPosition(std::numeric_limits<double>::infinity());
        wheel2->setPosition(std::numeric_limits<double>::infinity());
        wheel3->setPosition(std::numeric_limits<double>::infinity());
        wheel4->setPosition(std::numeric_limits<double>::infinity());
        wheel5->setPosition(std::numeric_limits<double>::infinity());
        wheel6->setPosition(std::numeric_limits<double>::infinity());
        wheel7->setPosition(std::numeric_limits<double>::infinity());
        wheel8->setPosition(std::numeric_limits<double>::infinity());
        
        wheel1->setVelocity(0);
        wheel2->setVelocity(0);
        wheel3->setVelocity(0);
        wheel4->setVelocity(0);
        wheel5->setVelocity(0);
        wheel6->setVelocity(0);
        wheel7->setVelocity(0);
        wheel8->setVelocity(0);        
    }
    
    virtual void setLeftSpeed(double speed) {
        wheel1->setVelocity(speed);
        wheel2->setVelocity(speed);
        wheel5->setVelocity(speed);
        wheel6->setVelocity(speed);
    }
    
    virtual void setRightSpeed(double speed) {
        wheel3->setVelocity(speed);
        wheel4->setVelocity(speed);
        wheel7->setVelocity(speed);
        wheel8->setVelocity(speed);
    }
    
protected:
    // Scout Wheels
	Servo *wheel1, *wheel2, *wheel5, *wheel6; // Left Track
	Servo *wheel3, *wheel4, *wheel7, *wheel8; // Right Track
};


#endif
